package com.rockwell.robot2015.model.solvers;

import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.sensors.IRobotSensors;

public class RightFirstMazeSolver implements IMazeSolver {

	@Override
	public void reset(Robot robot) {
	}
	
	@Override
	public void onJunctionReached(Robot robot) {
	}

	@Override
	public void onFrontWallReached(Robot robot) {
	}

	@Override
	public void move(Robot robot) {
		IRobotSensors sensor = robot.getSensor();
		
		if (sensor.detectLeftWall()) {
			robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
		}
		
		if (!sensor.detectRightWall()) {
			robot.sendCommand(RobotAction.MoveWithoutSensor,
					LEFT_TURN_CLEARANCE);
			robot.sleep(Robot.MOTION_SETTLING_TIME);

			robot.turnRight();
			robot.sleep(Robot.MOTION_SETTLING_TIME);
		}
		else if (!sensor.detectFrontWall()) {
			//robot.sendCommand(RobotAction.Jog2);
		}
		else if (!sensor.detectLeftWall()) {
			robot.sendCommand(RobotAction.MoveWithoutSensor, CORNER_CLEARANCE);
			robot.sleep(Robot.MOTION_SETTLING_TIME);
			
			robot.turnLeft();
			robot.sleep(Robot.MOTION_SETTLING_TIME);
		}
		else {
			robot.turnBack();
			robot.sleep(Robot.MOTION_SETTLING_TIME);
		}
	}

}
